/**
 * Created by bourne on 2024/4/20.
 *
 * 了在ROS（机器人操作系统）环境中提供一些感知工具功能，
 * 如设置位置和偏航角、获取当前姿态信息、检查点是否在视野
 * 范围内、获取视野范围的边界框等。 **/

#ifndef SRC_PERCEPTION_UTILS_H
#define SRC_PERCEPTION_UTILS_H

#include <ros/ros.h>
#include <Eigen/Eigen>
#include <iostream>
#include <memory>
#include <vector>

using Eigen::Vector3d;
using std::shared_ptr;
using std::unique_ptr;
using std::vector;

namespace fast_planner {
class PerceptionUtils {
public:
  PerceptionUtils(ros::NodeHandle& nh);
  ~PerceptionUtils();

  // Set position and yaw
  void setPose(const Vector3d& pos, const double& yaw); // 设置位置和偏航角

  // Get info of current pose
  void getFOV(vector<Vector3d>& list1, vector<Vector3d>& list2); // 获取当前姿态的fov信息
  bool insideFOV(const Vector3d& point); // 检查点是否在视野范围内
  void getFOVBoundingBox(Vector3d& bmin, Vector3d& bmax); // 获取视野范围的边界框
private:
  // Data
  // Current camera pos and yaw
  Vector3d pos_;
  double yaw_;
  // Camera plane's normals in world frame 摄影机平面在世界frame中的法线
  vector<Vector3d> normals_;

  /* Params */
  // Sensing range of camera 摄像头感应范围
  double left_angle_, right_angle_, top_angle_, max_dist_, vis_dist_; // 摄像头的感应范围
  // Normal vectors of camera FOV planes in camera frame
  Vector3d n_top_, n_bottom_, n_left_, n_right_; // 摄像头视野平面在摄像头坐标系中的法线向量
  // Transform between camera and body
  Eigen::Matrix4d T_cb_, T_bc_; // 摄像头坐标系和机器人本体坐标系之间的变换矩阵
  // FOV vertices in body frame
  vector<Vector3d> cam_vertices1_, cam_vertices2_; // 在机器人本体坐标系中视野顶点
};
}
#endif //SRC_PERCEPTION_UTILS_H
